home *** CD-ROM | disk | FTP | other *** search
/ AI Game Programming Wisdom / AIGameProgrammingWisdom.iso / SourceCode / 03 Pathfinding with Astar / 01 Matthews / ase / PathFinder.cpp < prev    next >
Encoding:
C/C++ Source or Header  |  2001-06-30  |  6.1 KB  |  320 lines
  1. //////////////////////////////////////////////////////////////////
  2. // Class:    CAStar class (27/6/2001)
  3. // File:    AStar.cpp
  4. // Author:    James Matthews
  5. //
  6. // Implements the A* algorithm.
  7. // 
  8. //
  9. // Please visit http://www.generation5.org/ for the latest
  10. // in Artificial Intelligence news, interviews, articles and
  11. // discussion forums.
  12. //
  13.  
  14. #include <math.h>
  15. #include "PathFinder.h"
  16.  
  17. CAStar::CAStar() 
  18. {
  19.     m_pOpen = m_pClosed = NULL;
  20.     m_pStack = NULL;
  21.     m_pBest = NULL;
  22.  
  23.     udCost = NULL;
  24.     udValid = NULL;
  25.     udNotifyChild = NULL;
  26.     udNotifyList = NULL;
  27. }
  28.  
  29. CAStar::~CAStar() 
  30. {
  31.     ClearNodes();
  32. }
  33.  
  34. void CAStar::ClearNodes() 
  35. {
  36.     _asNode *temp = NULL, *temp2 = NULL;
  37.  
  38.     if (m_pOpen) {
  39.         while (m_pOpen) {
  40.             temp = m_pOpen->next;
  41.  
  42.             delete m_pOpen;
  43.  
  44.             m_pOpen = temp;
  45.         }
  46.     }
  47.  
  48.     if (m_pClosed) {
  49.         while (m_pClosed) {
  50.             temp = m_pClosed->next;
  51.  
  52.             delete m_pClosed;
  53.  
  54.             m_pClosed = temp;
  55.         }
  56.     }
  57. }
  58.  
  59. /////////////////////////////////////////////////
  60. // CAStar::GeneratePath(int, int, int, int)
  61. //
  62. // Main A* algorithm. The step functions are used
  63. // to keep code redundancy to a minimum.
  64. //
  65.  
  66. bool CAStar::GeneratePath(int sx, int sy, int dx, int dy) 
  67. {
  68.     StepInitialize(sx, sy, dx, dy);
  69.     
  70.     int retval = 0;
  71.     while (retval == 0) {
  72.         retval = Step();
  73.     };
  74.     
  75.     if (retval == -1 || !m_pBest) {
  76.         m_pBest = NULL;
  77.         return false;
  78.     }
  79.  
  80.     return true;
  81. }
  82.  
  83. void CAStar::StepInitialize(int sx, int sy, int dx, int dy)
  84. {
  85.     ClearNodes();
  86.     
  87.     m_iSX = sx; m_iSY = sy; m_iDX = dx; m_iDY = dy;
  88.     m_iDNum = Coord2Num(dx,dy);
  89.  
  90.     _asNode *temp = new _asNode(sx, sy);
  91.  
  92.     temp->g = 0;
  93.     temp->h = abs(dx-sx) + abs(dy-sy);
  94.     temp->f = temp->g + temp->h;
  95.     temp->number = Coord2Num(sx, sy);
  96.  
  97.     m_pOpen = temp;
  98.  
  99.     udFunc(udNotifyList, NULL, m_pOpen, ASNL_STARTOPEN, m_pNCData);
  100.     udFunc(udNotifyChild, NULL, temp, 0, m_pNCData);
  101. }
  102.  
  103. int CAStar::Step()
  104. {
  105.     if (!(m_pBest = GetBest()))
  106.         return -1;
  107.  
  108.     if (m_pBest->number == m_iDNum) 
  109.         return 1;
  110.  
  111.     CreateChildren(m_pBest);
  112.  
  113.     return 0;
  114. }
  115.  
  116. _asNode *CAStar::GetBest() 
  117. {
  118.     if (!m_pOpen) return NULL;
  119.  
  120.     _asNode *temp = m_pOpen, *temp2 = m_pClosed;
  121.     m_pOpen = temp->next;
  122.  
  123.     udFunc(udNotifyList, NULL, temp, ASNL_DELETEOPEN, m_pNCData);
  124.  
  125.     m_pClosed = temp;
  126.     m_pClosed->next = temp2;
  127.  
  128.     udFunc(udNotifyList, NULL, m_pClosed, ASNL_ADDCLOSED, m_pNCData);
  129.  
  130.     return temp;
  131. }
  132.  
  133. void CAStar::CreateChildren(_asNode *node) 
  134. {
  135.     _asNode temp;
  136.     int x = node->x, y = node->y;
  137.  
  138.     for (int i=-1;i<2;i++) {
  139.         for (int j=-1;j<2;j++) {
  140.             temp.x = x+i;
  141.             temp.y = y+j;
  142.             if (i == 0 && j == 0 || !udFunc(udValid, node, &temp, 0, m_pCBData)) continue;
  143.  
  144.             LinkChild(node, &temp);
  145.         }
  146.     }
  147. }
  148.  
  149. void CAStar::LinkChild(_asNode *node, _asNode *temp) 
  150. {
  151.     int x = temp->x;
  152.     int y = temp->y;
  153.     int g = node->g + udFunc(udCost, node, temp, 0, m_pCBData);
  154.     int num = Coord2Num(x,y);
  155.  
  156.     _asNode *check = NULL;
  157.  
  158.     if (check = CheckList(m_pOpen, num)) {
  159.         node->children[node->numchildren++] = check;
  160.  
  161.         // A better route found, so update
  162.         // the node and variables accordingly.
  163.         if (g < check->g) {
  164.             check->parent = node;
  165.             check->g = g;
  166.             check->f = g + check->h;
  167.             udFunc(udNotifyChild, node, check, 1, m_pNCData);
  168.         } else {
  169.             udFunc(udNotifyChild, node, check, 2, m_pNCData);
  170.         }
  171.     } else if (check = CheckList(m_pClosed, num)) {
  172.         node->children[node->numchildren++] = check;
  173.  
  174.         if (g < check->g) {
  175.             check->parent = node;
  176.             check->g = g;
  177.             check->f = g + check->h;
  178.  
  179.             udFunc(udNotifyChild, node, check, 3, m_pNCData);
  180.  
  181.             // The fun part...
  182.             UpdateParents(check);
  183.         } else {
  184.             udFunc(udNotifyChild, node, check, 4, m_pNCData);
  185.         }
  186.     } else {
  187.         _asNode *newnode = new _asNode(x,y);
  188.         newnode->parent = node;
  189.         newnode->g = g;
  190.         newnode->h = abs(x-m_iDX) + abs(y-m_iDY);
  191.         newnode->f = newnode->g + newnode->h;
  192.         newnode->number = Coord2Num(x,y);
  193.  
  194.         AddToOpen(newnode);
  195.  
  196.         node->children[node->numchildren++] = newnode;
  197.  
  198.         udFunc(udNotifyChild, node, newnode, 5, m_pNCData);
  199.     }
  200. }
  201.  
  202. _asNode *CAStar::CheckList(_asNode *node, int num) 
  203. {
  204.     while (node) {
  205.         if (node->number == num) return node;
  206.  
  207.         node = node->next;
  208.     }
  209.  
  210.     return NULL;
  211. }
  212.  
  213. void CAStar::AddToOpen(_asNode *addnode) 
  214. {
  215.     _asNode *node = m_pOpen;
  216.     _asNode *prev = NULL;
  217.  
  218.     if (!m_pOpen) {
  219.         m_pOpen = addnode;
  220.         m_pOpen->next = NULL;
  221.  
  222.         udFunc(udNotifyList, NULL, addnode, ASNL_STARTOPEN, m_pNCData);
  223.  
  224.         return;
  225.     }
  226.  
  227.     while(node) {
  228.         if (addnode->f > node->f) {
  229.             prev = node;
  230.             node = node->next;
  231.         } else {
  232.             if (prev) {
  233.                 prev->next = addnode;
  234.                 addnode->next = node;
  235.                 udFunc(udNotifyList, prev, addnode, ASNL_ADDOPEN, m_pNCData);
  236.             } else {
  237.                 _asNode *temp = m_pOpen;
  238.  
  239.                 m_pOpen = addnode;
  240.                 m_pOpen->next = temp;
  241.                 udFunc(udNotifyList, temp, addnode, ASNL_STARTOPEN, m_pNCData);
  242.             }
  243.  
  244.             return;
  245.         }
  246.     }
  247.  
  248.     prev->next = addnode;
  249.     udFunc(udNotifyList, prev, addnode, ASNL_ADDOPEN, m_pNCData);
  250. }
  251.  
  252. void CAStar::UpdateParents(_asNode *node) 
  253. {
  254.     int g = node->g, c = node->numchildren;
  255.  
  256.     _asNode *kid = NULL;
  257.     for (int i=0;i<c;i++) {
  258.         kid = node->children[i];
  259.         if (g+1 < kid->g) {
  260.             kid->g = g+1;
  261.             kid->f = kid->g + kid->h;
  262.             kid->parent = node;
  263.             
  264.             Push(kid);
  265.         }
  266.     }
  267.  
  268.     _asNode *parent;
  269.     while (m_pStack) {
  270.         parent = Pop();
  271.         c = parent->numchildren;
  272.         for (int i=0;i<c;i++) {
  273.             kid = parent->children[i];
  274.             
  275.             if (parent->g+1 < kid->g) {
  276.                 kid->g = parent->g + udFunc(udCost, parent, kid, 0, m_pCBData);
  277.                 kid->f = kid->g + kid->h;
  278.                 kid->parent = parent;
  279.  
  280.                 Push(kid);
  281.             }
  282.         }
  283.     }
  284. }
  285.  
  286. void CAStar::Push(_asNode *node)
  287. {
  288.  
  289.     if (!m_pStack) {
  290.         m_pStack = new _asStack;
  291.         m_pStack->data = node;
  292.         m_pStack->next = NULL;
  293.     } else {
  294.         _asStack *temp = new _asStack;
  295.  
  296.         temp->data = node;
  297.         temp->next = m_pStack;
  298.         m_pStack = temp;
  299.     }
  300. }
  301.  
  302. _asNode *CAStar::Pop() 
  303. {
  304.     _asNode *data = m_pStack->data;
  305.     _asStack *temp = m_pStack;
  306.  
  307.     m_pStack = temp->next;
  308.     
  309.     delete temp;
  310.  
  311.     return data;
  312. }
  313.  
  314. int CAStar::udFunc(_asFunc func, _asNode *param1, _asNode *param2, int data, void *cb)
  315. {
  316.     if (func) return func(param1, param2, data, cb);
  317.  
  318.     return 1;
  319. }
  320.